#include "simpleode/lib/fing_001.h"
#include <iostream>
#include <sstream>
#include <iomanip>
#include <fstream>
#include <string>
#include <cmath>
#include <unistd.h>
using namespace std;


const bool CONFIG_PRINT_STATUS(false);

const int CONFIG_BALL_INDEX(3);


enum CTRL_STEP {
  CTRL_STEP_INIT,
  CTRL_STEP_REACH_POS_INIT,
  CTRL_STEP_WAIT_FOR_FIRE,
  CTRL_STEP_REACH_POS_TARGET,
  CTRL_STEP_DETECH_LANDING,
  CTRL_STEP_OUTPUT_RESULT,
  CTRL_STEP_WAIT_FOR_RESET,
  CTRL_STEP_RESET
};


void print_log(string log, double time)
{
  cout << "[ " << setprecision(3) << fixed << time << " ] ";
  cout << log << endl;
}


bool read_msg_controller(dReal& pos_init, dReal& pos_target, dReal& duration)
{
  ifstream msg_controller("msg_controller.msg");
  
  if (msg_controller.is_open())
  {
    dReal read_pos_init, read_pos_target, read_duration;
    bool hasRead = false;
    if (msg_controller >> read_pos_init >> read_pos_target >> read_duration)
    {
      while (msg_controller >> read_pos_init >> read_pos_target >> read_duration);
      pos_init    = read_pos_init;
      pos_target  = read_pos_target;
      duration    = read_duration;
      
      hasRead = true;
    }
    
    msg_controller.close();
    
    if (hasRead)
    {
      ofstream msg_controller_flush("msg_controller.msg");
      if (msg_controller_flush.is_open())
      {
        msg_controller_flush << "";
        msg_controller_flush.close();
      }
    }
    
    return hasRead;
  }
  
  return false;
}


bool write_msg_simulator(const dReal& loc_land)
{
  ofstream msg_simulator("msg_simulator.msg");
  
  if (msg_simulator.is_open())
  {
    msg_simulator << loc_land << endl;
    
    msg_simulator.close();
    return true;
  }
  
  return false;
}


void callback_ctrl(xode::TEnvironment &env, xode::TDynRobot &robot)
{
  static int step(CTRL_STEP_RESET);
  
  static dReal pos_init;
  static dReal pos_target;
  static dReal duration;
  
  static dReal loc_land;
  
  static double time_step_begin;
  
  // Collision detection
  bool collision_ball = robot.GetBodyContact(CONFIG_BALL_INDEX);
  
  // Position feedback
  dReal loc_x = robot.GetPositionX(CONFIG_BALL_INDEX);
  dReal loc_y = robot.GetPositionY(CONFIG_BALL_INDEX);
  dReal loc_z = robot.GetPositionZ(CONFIG_BALL_INDEX);
  dReal pos   = robot.GetAngleHinge(0);
  
  // Print status
  if (CONFIG_PRINT_STATUS)
  {
    stringstream ss;
    ss << "P_ball = (" << setprecision(6) << fixed << loc_x << ", " 
                       << setprecision(6) << fixed << loc_y << ", " 
                       << setprecision(6) << fixed << loc_z << "), " 
       << "C_ball = "  << (collision_ball ? "Y" : "N") << ", "
       << "pos = "     << setprecision(6) << fixed << pos;
    print_log(ss.str(), env.Time());
  }
  
  // Control sequence
  dReal set_omega = 0.0;
  switch (step)
  {
    case CTRL_STEP_INIT:
    {
      set_omega = 0.0;
      env.Reset();
      
      bool hasRead = read_msg_controller(pos_init, pos_target, duration);
      
      if (hasRead)
      {
        stringstream ss;
        ss << "pos_init = "   << setprecision(6) << fixed << pos_init   << ", " 
           << "pos_target = " << setprecision(6) << fixed << pos_target << ", " 
           << "duration = "   << setprecision(6) << fixed << duration;
        print_log(ss.str(), env.Time());
        
        step = CTRL_STEP_REACH_POS_INIT;
        print_log("CTRL_STEP_REACH_POS_INIT", env.Time());
        time_step_begin = env.Time();
      }
    }
    break;
    
    case CTRL_STEP_REACH_POS_INIT:
    {
      const dReal SPEED_REACH_POS_INIT(0.01 * M_PI);
      
      dReal gain = pos_init - pos;
      dReal speed = gain / abs(gain) * SPEED_REACH_POS_INIT;
      dReal speed_kp = 3.0 * gain;
      if (speed_kp < speed) speed = speed_kp;
      set_omega = speed;
      
      if (abs(pos - pos_init) < 0.000001 * M_PI)
      {
        set_omega = 0.0;
        step = CTRL_STEP_WAIT_FOR_FIRE;
        print_log("CTRL_STEP_WAIT_FOR_FIRE", env.Time());
        time_step_begin = env.Time();
      }
    }
    break;
    
    case CTRL_STEP_WAIT_FOR_FIRE:
    {
      const double TIME_WAIT_FOR_FIRE(1.0);
      
      set_omega = 0.0;
      
      if (env.Time() >= time_step_begin + TIME_WAIT_FOR_FIRE)
      {
        step = CTRL_STEP_REACH_POS_TARGET;
        print_log("CTRL_STEP_REACH_POS_TARGET", env.Time());
        time_step_begin = env.Time();
      }
    }
    break;
    
    case CTRL_STEP_REACH_POS_TARGET:
    {
      dReal speed = (pos_target - pos_init) / duration;
      set_omega = speed;

      if (pos >= pos_target)
      {
        set_omega = 0.0;
        step = CTRL_STEP_DETECH_LANDING;
        print_log("CTRL_STEP_DETECH_LANDING", env.Time());
        time_step_begin = env.Time();
      }
    }
    break;

    case CTRL_STEP_DETECH_LANDING:
    {
      const dReal MAX_LANDING_BALL_Z = 0.101;
      const double MAX_THROWING_TIME = 5.0;
      const dReal MIN_THROW_BALL_X = 0.25;

      set_omega = 0.0;

      if (env.Time() > time_step_begin + MAX_THROWING_TIME && loc_x < MIN_THROW_BALL_X)
      {
        stringstream ss;
        
        loc_land = loc_x;
        
        ss << "not thrown. " 
           << "loc_land = " << setprecision(6) << fixed << loc_land << ", " 
           <<           "(" << setprecision(6) << fixed << loc_x    << ", " 
                            << setprecision(6) << fixed << loc_y    << ", " 
                            << setprecision(6) << fixed << loc_z    << ")";
        print_log(ss.str(), env.Time());
        
        step = CTRL_STEP_OUTPUT_RESULT;
        print_log("CTRL_STEP_OUTPUT_RESULT", env.Time());
        time_step_begin = env.Time();
      }

      if (collision_ball && loc_z < MAX_LANDING_BALL_Z)
      {
        stringstream ss;
        
        loc_land = loc_x;
        
        ss << "loc_land = " << setprecision(6) << fixed << loc_land << ", " 
           <<           "(" << setprecision(6) << fixed << loc_x    << ", " 
                            << setprecision(6) << fixed << loc_y    << ", " 
                            << setprecision(6) << fixed << loc_z    << ")";
        print_log(ss.str(), env.Time());
        
        step = CTRL_STEP_OUTPUT_RESULT;
        print_log("CTRL_STEP_OUTPUT_RESULT", env.Time());
        time_step_begin = env.Time();
      }
    }
    break;

    case CTRL_STEP_OUTPUT_RESULT:
    {
      bool hasWritten = write_msg_simulator(loc_land);
      
      if (hasWritten)
      {
        step = CTRL_STEP_WAIT_FOR_RESET;
        print_log("CTRL_STEP_WAIT_FOR_RESET", env.Time());
        time_step_begin = env.Time();
      }
    }
    break;
    
    case CTRL_STEP_WAIT_FOR_RESET:
    {
      const double TIME_WAIT_FOR_RESET(1.0);
      
      if (env.Time() >= time_step_begin + TIME_WAIT_FOR_RESET)
      {
        step = CTRL_STEP_RESET;
        print_log("CTRL_STEP_RESET", env.Time());
        time_step_begin = env.Time();
      }
    }
    break;
    
    case CTRL_STEP_RESET:
    {
      set_omega = 0.0;
      env.Reset();
      
      step = CTRL_STEP_INIT;
      print_log("CTRL_STEP_INIT", env.Time());
      time_step_begin = env.Time();
    }
    break;
  }
  
  robot.SetAngVelHinge(0, set_omega);
  //usleep(unsigned(env.TimeStep() * 1e6));
}


void callback_draw(xode::TEnvironment &env, xode::TDynRobot &robot)
{
}


void callback_keyevent(xode::TEnvironment &env, xode::TDynRobot &robot, int command)
{
  if (command == 'r') env.Reset();
}


int main (int argc, char **argv)
{
  xode::TEnvironment env;
  env.TimeStep(0.02);
  
  dsFunctions fn = xode::SimInit("textures", env);
  
  xode::ControlCallback = &callback_ctrl;
  xode::DrawCallback = &callback_draw;
  xode::KeyEventCallback = &callback_keyevent;
  
  dsSimulationLoop(argc, argv, 400, 400, &fn);
  
  dCloseODE();
  return 0;
}
